// #include "map_base.h"
#include <mc_map/map_base.h>

// 设置地图数据
void MapBase::setOccupancyGrid(const OccupancyGridPtr& occupancy_grid_ptr){
    occupancyGrid_ = occupancy_grid_ptr;

    // 地图元数据信息
    resolution_ = occupancy_grid_ptr->info.resolution;
    width_ = occupancy_grid_ptr->info.width;
    height_ = occupancy_grid_ptr->info.height;
    origin_ = Vec2f(occupancy_grid_ptr->info.origin.position.x,
        occupancy_grid_ptr->info.origin.position.y);
    mapSize_ = width_ * height_;
    frameId_ = occupancy_grid_ptr->header.frame_id;
    
    // 转map
    cvMap_ = cv::Mat::zeros(height_, width_, CV_8UC1);
    for(int y = 0; y < height_; y++) {
        for(int x = 0; x < width_; x++) {
            int8_t occupancy_value = occupancy_grid_ptr->data[y * width_ + x];
            //反转y轴坐标 (ros中原点在左下角，opencv中原点在左上角)
            int flipped_y = height_ - 1 - y; 
            if(occupancy_value > 70){
                cvMap_.at<uchar>(flipped_y, x) = 0;
            }else if(occupancy_value < 30 && occupancy_value >= 0){
                cvMap_.at<uchar>(flipped_y, x) = 255;
            }
            else{
                cvMap_.at<uchar>(flipped_y, x) = 127;
            }
        }
    }

    LOG(INFO) << "Map Info:";
    LOG(INFO) << "  Resolution: " << occupancy_grid_ptr->info.resolution << " meters per cell";
    LOG(INFO) << "  Width: " << occupancy_grid_ptr->info.width 
            << " *  Height: " << occupancy_grid_ptr->info.height << " cells";
    LOG(INFO) << "  Origin (x, y, z): ("
              << occupancy_grid_ptr->info.origin.position.x << ", "
              << occupancy_grid_ptr->info.origin.position.y << ", "
              << occupancy_grid_ptr->info.origin.position.z << ")";
    LOG(INFO) << "  Total grid cells: " << occupancy_grid_ptr->info.width * occupancy_grid_ptr->info.height;
}


// 世界坐标转像素坐标
Vec2i MapBase::WorldToPixel(const Vec2f& point){
    Vec2i pixel_point(
        std::round((point.x - origin_.x) / resolution_), 
        std::round((point.y - origin_.y) / resolution_));
    return pixel_point;
}

// 世界坐标转像素坐标
Vec2i MapBase::WorldToOpencvPixel(const Vec2f& point){
    Vec2i pixel_point(
        std::round((point.x - origin_.x) / resolution_), 
        std::round((point.y - origin_.y) / resolution_));
    pixel_point.y = height_ - 1 - pixel_point.y; 
    return pixel_point;
}


// 像素坐标转世界坐标
Vec2f MapBase::OpencvPixelToWorld(const Vec2i& point){
    Vec2f world_point(
        origin_.x + (point.x + 0.5) * resolution_, 
        origin_.y + (height_ - 1 - point.y + 0.5) * resolution_
    );
    return world_point;
}


// 像素坐标转世界坐标
Vec2f MapBase::PixelToWorld(const Vec2i& point){
    Vec2f world_point(
        origin_.x + (point.x + 0.5) * resolution_, 
        origin_.y + (point.y + 0.5) * resolution_
    );
    return world_point;
}

// 二维数据转一维
size_t MapBase::ToIndex(Vec2i pixel_2d){
    return pixel_2d.y * width_ + pixel_2d.x;
}


bool MapBase::IsValid(Vec2i pixel_2d){
    if(pixel_2d.x < 0 || pixel_2d.x >= width_)
        return false;
    if(pixel_2d.y < 0 || pixel_2d.y >= height_)
        return false;
    return true;
}

bool MapBase::IsValid(size_t idx){
    if(idx < 0 || idx >= mapSize_)
        return false;
    return true;
}

// 获取像素值，包含边界检测
bool MapBase::GetValue(Vec2i pixel_2d, int& value){
    if(IsValid(pixel_2d)){
        value = cvMap_.at<uchar>(pixel_2d);
        return true;
    }
    return false;
}

// 数值是否有效
bool MapBase::ValueIsValid(Vec2i pixel_2d){
    int value;
    if(GetValue(pixel_2d, value)){
        // cout << "value: " << value << endl;
        if(value < 200) 
            return false;
        else
            return true;
    }
    return false;
}

// 简单膨胀处理
void MapBase::SimpleDilateOccupancyGrid(float robot_radius){
    cv::Mat map_image(height_, width_, CV_8UC1); 
    // for (int i = 0; i < __map_ptr->data.size(); ++i) {
    for (int y = 0; y < height_; ++y) {
        for (int x = 0; x < width_; ++x){
            int index = y * width_ + x;
            if(index < 0 || index >= occupancyGrid_->data.size()) continue;
            int8_t value = occupancyGrid_->data[index];
            uchar pixel_value = static_cast<uchar>(value < 0 ? 128 : (255 - value / 100.0 * 255)); 
            map_image.at<uchar>(y, x) = pixel_value;     
        } 
    }
    cv::Mat map_dilate;
    cv::flip(map_image, map_dilate, 0);    //上下翻转    
    // 障碍物膨胀处理
    int robot_radius_pixel = int(robot_radius / resolution_);
    cv::Mat dilate_struct = cv::getStructuringElement(0, cv::Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(map_dilate, map_dilate, dilate_struct);
    cvMap_ = map_dilate;
}

void MapBase::SimpleDilateOpencvMap(float robot_radius){
    // 障碍物膨胀处理
    int robot_radius_pixel = int(robot_radius / resolution_);
    cv::Mat dilate_struct = cv::getStructuringElement(0, cv::Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(cvMap_, cvMap_, dilate_struct);
}


// 获取地图数据
bool MapBase::getMap(cv::Mat& cv_map){
    if(cvMap_.empty()) return false;
    cv_map = cvMap_.clone();
    return true;
}

bool MapBase::getMap(nav_msgs::OccupancyGrid::Ptr& map_ptr, string frame_id){
    if(cvMap_.empty()) return false;
    map_ptr->data.resize(width_ * height_);
    // cout << "-----------------------------------------" << endl;
    for(int w = 0; w < cvMap_.cols; w++) {
        for(int h = 0; h < cvMap_.rows; h++) {
            if(cvMap_.at<uchar>(h, w) == 0){
                int flipped_h = cvMap_.rows - 1 - h;   //反转y轴坐标 (ros中原点在左下角，opencv中原点在左上角)
                size_t idx = flipped_h * width_ + w;
                if(idx <0 || idx > map_ptr->data.size()) continue;
                map_ptr->data[idx] = 100;
            }       
        }
    }
    map_ptr->info = occupancyGrid_->info;
    map_ptr->header.stamp = ros::Time::now();
    map_ptr->header.frame_id = frame_id;

    // map_ptr->info.resolution = resolution_;
    // map_ptr->info.width = cvMap_.cols;
    // map_ptr->info.height = cvMap_.rows;
    // map_ptr->info.origin.position.x = -map_ptr->info.width / 2.0;
    // map_ptr->info.origin.position.y = -map_ptr->info.height / 2.0;
    // map_ptr->header.stamp = ros::Time::now();
    // map_ptr->header.frame_id = frame_id;
    return true;
}